/*
所有上层主函数
DIY：https://item.taobao.com/item.htm?spm=a1z10.1-c.w137644-14146616909.40.TkqpM3&id=544751376675
*/


#include "bcomdef.h"
#include "OSAL.h"
#include "OSAL_PwrMgr.h"

#include "OnBoard.h"
#include "hal_adc.h"
#include "hal_led.h"
#include "hal_key.h"
#include "hal_lcd.h"

#include "gatt.h"

#include "hci.h"

#include "gapgattserver.h"
#include "gattservapp.h"
#include "devinfoservice.h"
#include "simpleGATTprofile.h"

#if defined( CC2540_MINIDK )
  #include "simplekeys.h"
#endif

#if defined ( PLUS_BROADCASTER )
  #include "peripheralBroadcaster.h"
#else
  #include "peripheral.h"
#endif

#include "gapbondmgr.h"


#include "bob_lib/bob.h"


#define duoji_A	P1_2



#define left_lower	P0_2  /*L*/
#define left_upper	P0_3   /*l*/
#define right_lower	P0_4   //R
#define right_upper	P0_5   //r

#define motor_Set(pin_d, mode) pin_d=mode

#define max_rebuf  20
	  
#define max_num     1
#define max_servo   4





  // receive form server data
  typedef struct 
  {
	  int receive_len;
	  int receive_status; /*1  : idle,0:receiving,2,wait process*/
	  char	 receive_buf[max_rebuf];
  
  } Receive_D;
  
  
  
  typedef struct 
  {
	  int  x;
	  int  x_n;
	  int  x_old; /* */
	  unsigned int	x_count;
	  //unsigned char  y_count;
	  int xn;
	  int led;
	  //int yn;

  
  } servo_motor_status;

  unsigned int	  pwm;
  unsigned int count;	   //0.5ms 
  unsigned char jd; 		// 
  Receive_D  Receive_CMD[max_num];
  servo_motor_status	servo_motor[max_servo];
  int idle_buf=0;
  
void GPIO_GPIOInit()
{
	int i=0;
	/*先关所有led，防止初始化时led闪烁*/
	duoji_A =1; 
	/*设置LEDs引脚为输出*/	
	P0DIR |= BV(2)|BV(3)|BV(4)|BV(5);
	/*设置LEDs引脚为GPIO引脚*/	 
	P0SEL &= ~(BV(0)|BV(1));
	//  P1SEL = 0; // Configure Port 1 as GPIO
	duoji_A =1; 
	for(i=0;i<max_servo;i++)
	{
		servo_motor[i].x=5;
		servo_motor[i].x_old=5;
		servo_motor[i].x_n=0;
	}
}
  
 
void cmd_pase(char *data_c,int len)
{
 	int i;
	int j=0,tmp=0;

	for(i=0;i<len;i++)
	{
	    if(data_c[i]=='r')	   //  servo motor    r=right lower;R=right_upper;l=left_lower ;L=Left upper     
		{
		 	servo_motor[0].x=  atoi(data_c+i+1);
			/*servo_motor[0].x_n=servo_motor[0].x-servo_motor[0].x_old;
			servo_motor[0].x_old=servo_motor[0].x;
			servo_motor[0].x_count=0;
			servo_motor[0].xn=0;	
			*/
			set_dutycycle_r(servo_motor[0].x);
			break;
		}
	    else if(data_c[i]=='R')	   //  servo motor     
		{
		 	servo_motor[1].x=  atoi(data_c+i+1);
			/*servo_motor[1].x_n=servo_motor[1].x-servo_motor[1].x_old;
			servo_motor[1].x_old=servo_motor[1].x;
			servo_motor[1].x_count=0;
			servo_motor[1].xn=0;	
			*/
			set_dutycycle_R(servo_motor[1].x);
			break;
		}
	    else if(data_c[i]=='l')	   //  servo motor     
		{
		 	servo_motor[2].x=  atoi(data_c+i+1);
			/*servo_motor[2].x_n=servo_motor[2].x-servo_motor[2].x_old;
			servo_motor[2].x_old=servo_motor[2].x;
			servo_motor[2].x_count=0;
			servo_motor[2].xn=0;	 
			*/
			set_dutycycle_l(servo_motor[2].x);
			break;
		}
	    else if(data_c[i]=='L')	   //  servo motor     
		{
		 	servo_motor[3].x=  atoi(data_c+i+1);
			/*servo_motor[3].x_n=servo_motor[3].x-servo_motor[3].x_old;
			servo_motor[3].x_old=servo_motor[3].x;
			servo_motor[3].x_count=0;
			servo_motor[3].xn=0;	 
			*/
			set_dutycycle_L(servo_motor[3].x);
			break;
		}
			
	}
}

void BT_receive_callback(char datar)
{

	if(datar==';')
	{
		Receive_CMD[0].receive_buf[Receive_CMD[0].receive_len++] = 0;	
		Receive_CMD[0].receive_status=2;
		idle_buf++;
		/////
		cmd_pase(Receive_CMD[0].receive_buf,Receive_CMD[0].receive_len);
		Receive_CMD[0].receive_status=0;
		Receive_CMD[0].receive_len=0;
		
		
	}
	else	
	{
		Receive_CMD[0].receive_buf[Receive_CMD[0].receive_len++] = datar;	
		Receive_CMD[0].receive_status=1;
		if(Receive_CMD[0].receive_len>max_rebuf) 
			Receive_CMD[0].receive_len=0;

	}

        
}


void Pin_set(char num,char value)
{
	if(num==0)
		motor_Set(left_lower, value);
	else if(num==1)
		motor_Set(left_upper, value);
	else if(num==2)
		motor_Set(right_lower, value);
	else if(num==3)
		motor_Set(right_upper, value);
		

}

void servo_control(char num)
{
      //  if(servo_motor.xn<50)  
    char x_tmp=0;
	servo_motor[num].x_count++;
	if(((servo_motor[num].x_count )==100)&&(servo_motor[num].xn<50))
	{
		//duoji_A=1;
		//motor_Set(duoji_A, 1);
		Pin_set(num,1);  
			
	 	servo_motor[num].x_count=0;
		servo_motor[num].xn++;
		x_tmp=servo_motor[num].xn/5;
		if(x_tmp<=servo_motor[num].x_n) 
			servo_motor[num].x=servo_motor[num].x_old+x_tmp;
	}
	else if(servo_motor[num].x==servo_motor[num].x_count)
		Pin_set(num,0); //motor_Set(duoji_A, 0);

	else if(servo_motor[num].xn>=50)
	{
		Pin_set(num,1); //motor_Set(duoji_A, 1);
		servo_motor[num].x=0;
	}

}


uint32 counter = 0;
uint8  timeout = 0;


/*中断函数:中断5次为1ms*/
#pragma vector = T3_VECTOR 
 __interrupt void T3_ISR(void) 
{ 
	int i=0;
	IRCON &= ~0x08;	   //定时器3中断标志位清0
	TIMIF &= ~0x01; 	  //定时器3溢出中断标志位清0
	for(i=0;i<max_servo;i++)
	{
		if(servo_motor[i].x)
			servo_control(i);
		else
		{
			Pin_set(i,1);//motor_Set(duoji_A, 1);
		}
	}
	T3CTL |= 0x10;        //再次启动定时器3

} 


void setup()
{
	pwm_init();
	set_pwm_period(20);//50hz  20ms
	pwm_start();
#if 0
  int i=0;
	////////////////////////
	//memmset(cmd_st,0,sizeof(cmd_st))
	for(i=0;i<max_num;i++) 
	{
		Receive_CMD[i].receive_status=0;
		Receive_CMD[i].receive_len =0;
	}
	 
	jd=4;	 
	idle_buf=0;
	///////////////
	GPIO_GPIOInit();
	/*定时器初始化*/
	T3CTL = 0xAE;	//32分频，中断使能，模模式
	T3CC0 = 0xC8;	// 1Mhz==1us  200us中断
	T3CCTL0 = 0x44;//一定要设置比较的方式，不然不会中断的!!!,T3CCTL0.IM==1,T3CCTL0.MODE==1
	/*中断使能*/
	EA = 1;
  	T3IE = 1;
	/*开始计时*/
	T3CTL |= 0x10; 
#endif
}


/**************************************************************************************************
 * @fn          main
 *
 * @brief       Start of application.
 *
 * @param       none
 *
 * @return      none
 **************************************************************************************************
 */
int main(void)
{
	unsigned int i=0,ii=0;
 	osal_device_init();
 	setup();
	/* Start OSAL */
	//HCI_EXT_HaltDuringRfCmd( HCI_EXT_HALT_DURING_RF_DISABLE);
	while(1)   
	{
		osal_start_system(); 
		
	}
  
}

